/***************************************************************************
 创建者: 华磊
 开始时间: 2020.8.14
 copyright: (C) 华友高科
 修改说明: (每次有修改就添加一条，带有 修改人，修改时间，修改描述)
 example (1) hualei 2020.5.8 类的具体实现代码编写

 ***************************************************************************/
#include "vehiclejogcontrol.h"
#include <QVariant>
#include <QSettings>
#include <QDebug>
#include <math.h>
#include "filteringalgorithm.h"
#include "motionmessage.h"
#include "axisobject.h"
#include "vehiclevelocitysmoother.h"
#include <QFile>
#include "imudevice.h"

#define D_READ_MOVE_LIMIT_RATIO 1.25

VehicleJogControl::VehicleJogControl(QString filePathIn, MotionMessage* motionMessageIn, int robotIdIn, double deltaTimeIn)
{
    configFilePath=filePathIn;
    motionMessage=motionMessageIn;
    robotId=robotIdIn;
    deltaTime=deltaTimeIn;
    vehicleVelJogCommand.z_rotate=0;
    vehicleVelJogCommand.x_move=0;
    vehicleVelJogCommand.y_move=0;

    vehicleVelAutoCommand.z_rotate=0;
    vehicleVelAutoCommand.x_move=0;
    vehicleVelAutoCommand.y_move=0;

    vehicleVelRosCommand.z_rotate=0;
    vehicleVelRosCommand.x_move=0;
    vehicleVelRosCommand.y_move=0;

    vehicleVelFeedback.z_rotate=0;
    vehicleVelFeedback.x_move=0;
    vehicleVelFeedback.y_move=0;

    vehiclePositionFeedback.rotate=0;
    vehiclePositionFeedback.x=0;
    vehiclePositionFeedback.y=0;

    vehicleVelJogCommand_beforeFilter.z_rotate=0;
    vehicleVelJogCommand_beforeFilter.x_move=0;
    vehicleVelJogCommand_beforeFilter.y_move=0;

    jogAcceleration_line=8;
    jogAcceleration_rotate=3;
    jogSafeRatio=1.0;



//    filtering_odom_vx=new FilteringAlgorithm;
//    filtering_odom_vy=new FilteringAlgorithm;
//    filtering_odom_vr=new FilteringAlgorithm;

    filtering_safeRatio=new FilteringAlgorithm;
    filtering_jog_x=new FilteringAlgorithm;
    filtering_jog_y=new FilteringAlgorithm;
    filtering_jog_r=new FilteringAlgorithm;

    velocitySmoother=NULL;


    initialFlag=false;
    timeStampInitialed=false;
    status=E_VEHICLE_STATUS_STOPPED;
    mode=E_VEHICLE_MODE_JOG;
    QString readComent;
    if(1==readConfigFile(filePathIn,readComent))
    {
        initialFlag=true;
        velocitySmoother=new VehicleVelocitySmoother(deltaTimeIn,filter_maxVel_line,filter_maxAcc_line,
                             filter_maxVel_line,filter_maxAcc_line,filter_maxVel_rotate,filter_maxAcc_rotate,filter_decel_factor);

    }
    else
    {
        addMsg(ENUM_MSG_ERROR,"MotionServer","VehicleJogControl",22005,robotId,0,0,0,0,readComent);
    }
    vehicleKinematic=new VehicleKinematics(vehicletype,wheelDiameter*correctionFactor_wheelDiameter
                                           ,wheelDistance*correctionFactor_wheelDistance
                                           ,wheelDistance_height*correctionFactor_wheelDistance_height
                                           ,steeringKp,wheelMaxSpeed,maxSteerAdjust);
    initialSteeringAxis();

    //imu
    QString imuConfigPath=D_TCRCONTROLLER_DATA_PATH;
    imuConfigPath+=D_IMU_DEVICE_CONFIG_PATH;
    imuDevice=new ImuDevice(imuConfigPath);
    if(false==imuDevice->isInitialOk())
    {
        initialFlag=false;
        addMsg(ENUM_MSG_ERROR,"MotionServer","VehicleJogControl",22006,robotId);
    }
}

int VehicleJogControl::getVehicleMovingCaligrateInfo(VehicleMovingCaligrateInfo &infoOut)
{
    infoOut.correctionFactor_wheelDiameter=correctionFactor_wheelDiameter;
    infoOut.correctionFactor_wheelDistance=correctionFactor_wheelDistance;
    infoOut.correctionFactor_wheelDistance_height=correctionFactor_wheelDistance_height;
    return 1;
}

int VehicleJogControl::setCalibrateWheelDiameter(double idealMovedLength, double actualMovedLength)
{
    double tmpCoo;
    switch(vehicletype)
    {
    case E_VEHICLE_TYPE_TWO_DIFFERENTIAL_DRIVE:
    {
        tmpCoo=actualMovedLength/idealMovedLength;
        correctionFactor_wheelDiameter=correctionFactor_wheelDiameter*tmpCoo;
        break;
    }
    case E_VEHICLE_TYPE_MCCRUM_4_WHEEL:
    {
        qDebug()<<"setCalibrateWheelDiameter ,unsupport E_VEHICLE_TYPE_MCCRUM_4_WHEEL";
        return -1;
        break;
    }
    case E_VEHICLE_TYPE_ONE_STERRING_WHEEL:
    {
        tmpCoo=actualMovedLength/idealMovedLength;
        correctionFactor_wheelDiameter=correctionFactor_wheelDiameter*tmpCoo;
        break;
    }
        default:
        {
        qDebug()<<"setCalibrateWheelDiameter ,unsupport "<<vehicletype;
        return -1;
        }
    }
    vehicleKinematic->resetKinematicPara(wheelDiameter*correctionFactor_wheelDiameter
                                           ,wheelDistance*correctionFactor_wheelDistance
                                           ,wheelDistance_height*correctionFactor_wheelDistance_height);
    //写文件
    writeConfigFile(configFilePath, correctionFactor_wheelDiameter,
                                           correctionFactor_wheelDistance,
                                           correctionFactor_wheelDistance_height);
    return 1;

}

int VehicleJogControl::setCalibrateWheelDistance(double idealMovedAngle, double actualMovedAngle)
{
    double tmpCoo;
    switch(vehicletype)
    {
    case E_VEHICLE_TYPE_TWO_DIFFERENTIAL_DRIVE:
    {
        tmpCoo=idealMovedAngle/actualMovedAngle;
        correctionFactor_wheelDistance=correctionFactor_wheelDistance*tmpCoo;
        break;
    }
    case E_VEHICLE_TYPE_MCCRUM_4_WHEEL:
    {
        qDebug()<<"setCalibrateWheelDiameter ,unsupport E_VEHICLE_TYPE_MCCRUM_4_WHEEL";
        return -1;
        break;
    }
    case E_VEHICLE_TYPE_ONE_STERRING_WHEEL:
    {
        tmpCoo=idealMovedAngle/actualMovedAngle;
        correctionFactor_wheelDistance_height=correctionFactor_wheelDistance_height*tmpCoo;
        break;
    }
        default:
        {
        qDebug()<<"setCalibrateWheelDiameter ,unsupport "<<vehicletype;
        return -1;
        }
    }
    vehicleKinematic->resetKinematicPara(wheelDiameter*correctionFactor_wheelDiameter
                                           ,wheelDistance*correctionFactor_wheelDistance
                                           ,wheelDistance_height*correctionFactor_wheelDistance_height);
    //写文件
    writeConfigFile(configFilePath, correctionFactor_wheelDiameter,
                                           correctionFactor_wheelDistance,
                                           correctionFactor_wheelDistance_height);
    return 1;
}

int VehicleJogControl::getImuDeviceData(ImuDataInfo &dataOut)
{
    return imuDevice->getImuDeviceData(dataOut);
}

int VehicleJogControl::getVehicleKinematicDimension(VehicleKinematicDimension &infoOut)
{

    infoOut.diameter=wheelDiameter;
    infoOut.wa=wheelDistance;
    infoOut.wb=wheelDistance_height;
    qDebug()<<"diameter"<<infoOut.diameter<<"wa"<<infoOut.wa<<"wb"<<infoOut.wb;
    return 1;
}

int VehicleJogControl::setVehicleKinematicDimension(VehicleKinematicDimension infoIn, int &returnFlag)
{

    returnFlag=0;
    if( !QFile::exists(configFilePath) )
    {
        // MessageLog::addMessage(" ");
        //消息预留
        //获取默认程序文件失败：文件不存在或路径错误
        qDebug()<< "setVehicleKinematicDimension error" ;
        return -1;
    }
    QSettings settings(configFilePath,QSettings::IniFormat);
    settings.setIniCodec("UTF-8");
    settings.setValue("/MechanicalInfo/wheelDiameter", infoIn.diameter);
    settings.setValue("/MechanicalInfo/wheelDistance", infoIn.wa);
    settings.setValue("/MechanicalInfo/wheelDistance_height", infoIn.wb);

    returnFlag=1;
    return 1;
}

int VehicleJogControl::getVehicleJogControlParameter(VehicleJogControlParameter &infoOut)
{
    infoOut.maxVel_x=vehicleMaxVel.x_move;
    infoOut.maxVel_y=vehicleMaxVel.y_move;
    infoOut.maxVel_r=vehicleMaxVel.z_rotate;
    return 1;
}

int VehicleJogControl::setVehicleJogControlParameter(VehicleJogControlParameter infoIn, int &returnFlag)
{

    returnFlag=0;
    if( !QFile::exists(configFilePath) )
    {
        // MessageLog::addMessage(" ");
        //消息预留
        //获取默认程序文件失败：文件不存在或路径错误
        qDebug()<< "setVehicleKinematicDimension error" ;
        return -1;
    }
    QSettings settings(configFilePath,QSettings::IniFormat);
    settings.setIniCodec("UTF-8");
    settings.setValue("/MechanicalInfo/maxSpeedX", infoIn.maxVel_x);
    settings.setValue("/MechanicalInfo/maxSpeedY", infoIn.maxVel_y);
    settings.setValue("/MechanicalInfo/maxSpeedRotation", infoIn.maxVel_r);

    returnFlag=1;
}



int VehicleJogControl::setSteeringControlKp(double kpIn, double maxSteerAdjustIn)
{
      return vehicleKinematic->setSteeringControlKp(kpIn,maxSteerAdjustIn);
}

int VehicleJogControl::setDebugFlag(int flagIn)
{
    return vehicleKinematic->setDebugFlag(flagIn);
}

int VehicleJogControl::setJogSafeRatio(double ratioIn)
{
    jogSafeRatio=filtering_safeRatio->arithmeticMeanfilter(ratioIn,9);//遇到障碍物容易一抖一抖的，所以加滤波。
//    jogSafeRatio=ratioIn;
    return 1;
}

int VehicleJogControl::resetAll()
{
//    qDebug()<<"VehicleJogControl::resetAll()";
    status=E_VEHICLE_STATUS_STOPPED;
    mode=E_VEHICLE_MODE_JOG;
    vehicleVelJogCommand.z_rotate=0;
    vehicleVelJogCommand.x_move=0;
    vehicleVelJogCommand.y_move=0;

    vehicleVelJogCommand_beforeFilter.z_rotate=0;
    vehicleVelJogCommand_beforeFilter.x_move=0;
    vehicleVelJogCommand_beforeFilter.y_move=0;

    vehicleVelAutoCommand.z_rotate=0;
    vehicleVelAutoCommand.x_move=0;
    vehicleVelAutoCommand.y_move=0;


    vehicleVelRosCommand.z_rotate=0;
    vehicleVelRosCommand.x_move=0;
    vehicleVelRosCommand.y_move=0;

//    vehicleVelFeedback.z_rotate=0;
//    vehicleVelFeedback.x_move=0;
//    vehicleVelFeedback.y_move=0;

//    vehiclePositionFeedback.rotate=0;
//    vehiclePositionFeedback.x=0;
//    vehiclePositionFeedback.y=0;

//    filtering_odom_vx->resetAll();
//    filtering_odom_vy->resetAll();
//    filtering_odom_vr->resetAll();

    filtering_safeRatio->resetAll();
    filtering_jog_x->resetAll();
    filtering_jog_y->resetAll();
    filtering_jog_r->resetAll();


    steeringAxis->positionFeedback=steeringAngleFeedback[0];
    steeringAxis->resetAll();
    isInSteeringSmooth=false;


    return 1;
}

int VehicleJogControl::stop()
{
//    qDebug()<<"stop,,,VehicleJogControl";
    status=E_VEHICLE_STATUS_STOPPED;
    resetAll();
    return 1;
}

int VehicleJogControl::startRun()
{
//    qDebug()<<"startRun,,,VehicleJogControl";
    status=E_VEHICLE_STATUS_RUNNING;
    return 1;
}

bool VehicleJogControl::isInitialOk()
{
    return initialFlag;
}



int VehicleJogControl::vehicleJog(double xMove,double yMove, double rotate)
{
    if(false==initialFlag)
    {
        return -1;
    }

    mode=E_VEHICLE_MODE_JOG;
    vehicleVelJogCommand_beforeFilter.z_rotate=vehicleMaxVel.z_rotate*rotate;
    vehicleVelJogCommand_beforeFilter.x_move=vehicleMaxVel.x_move*xMove;
    vehicleVelJogCommand_beforeFilter.y_move=vehicleMaxVel.y_move*yMove;

    velocitySmoother->setRawVelocityCommand(vehicleVelJogCommand_beforeFilter);

    //只判断静止后的第一次运动是否有转角突变。单舵轮todo

//    qDebug()<<"vehicleVelJogCommand_beforeFilter.x_move"<<vehicleVelJogCommand_beforeFilter.x_move
//              <<"y_move"<<vehicleVelJogCommand_beforeFilter.y_move
//             <<"z_rotate"<<vehicleVelJogCommand_beforeFilter.z_rotate;

//    qDebug()<<"vehicleVelJogCommand.x_move"<<vehicleVelJogCommand.x_move
//              <<"y_move"<<vehicleVelJogCommand.y_move
//             <<"z_rotate"<<vehicleVelJogCommand.z_rotate;

    return 1;

}

int VehicleJogControl::vehicleAuto(double xMove, double yMove, double rotate)
{
    mode=E_VEHICLE_MODE_AUTO;
    vehicleVelAutoCommand.x_move=xMove;
    vehicleVelAutoCommand.y_move=yMove;
    vehicleVelAutoCommand.z_rotate=rotate;
    velocitySmoother->setRawVelocityCommand(vehicleVelAutoCommand);
    if(rotate>8)
    {
        qDebug()<<"error rotate"<<rotate;//
    }

    return 1;
}

int VehicleJogControl::setRosVelCommand(double xMove, double yMove, double rotate)
{
    mode=E_VEHICLE_MODE_AUTO;
    vehicleVelRosCommand.x_move=xMove;
    vehicleVelRosCommand.y_move=yMove;
    vehicleVelRosCommand.z_rotate=rotate;
    velocitySmoother->setRawVelocityCommand(vehicleVelRosCommand);
    if(rotate>8)
    {
        qDebug()<<"setRosVelCommand error rotate2"<<rotate;//
    }

    return 1;
}

//int VehicleJogControl::calculateRosVelCommandSmooth()
//{
//    //将vehicleVelRosCommand平滑为vehicleVelAutoCommand
//    if(NULL!=velocitySmoother)
//    {
//        velocitySmoother->setAgvVelFeedback(vehicleVelFeedback);
//        velocitySmoother->calculateSmootherVelocityCommand(vehicleVelAutoCommand);
//    }

//    return 1;

//}

int VehicleJogControl::getVehicleVelCommand(bool isJogCommand, double &xMove, double &yMove, double &rotate)
{
    if(true==isJogCommand)
    {
        xMove=vehicleVelJogCommand.x_move;
        yMove=vehicleVelJogCommand.y_move;
        rotate=vehicleVelJogCommand.z_rotate;

    }
    else
    {
        xMove=vehicleVelAutoCommand.x_move;
        yMove=vehicleVelAutoCommand.y_move;
        rotate=vehicleVelAutoCommand.z_rotate;

    }

    return 1;
}

int VehicleJogControl::setVirtualLineTrack_feedfowardOutput(double feedforward_xIn, double feedforward_yIn,
                                                            double feedforward_rIn)
{
//    feedforward_x=feedforward_xIn;
//    feedforward_y=feedforward_yIn;
//    feedforward_r=feedforward_rIn;
//    return 1;
    return vehicleKinematic->setVirtualLineTrack_feedfowardOutput(feedforward_xIn,feedforward_yIn,feedforward_rIn);

}

int VehicleJogControl::moveLoop(QVector<double>& wheelVel,QVector<double> &steeringAngleOut)
{
    if(false==initialFlag)
    {
        return -1;
    }

    jogFitteringLoop();

    if(E_VEHICLE_STATUS_RUNNING!=status)
    {
//        qDebug()<<"info,,,VehicleJogControl::moveLoop, E_VEHICLE_STATUS_RUNNING!=status";
        stopVel(wheelVel);
        return -10;
    }

    if(E_VEHICLE_MODE_AUTO==mode)
    {
        if(vehicleVelAutoCommand.x_move<-vehicleRearRunSpeedLimit*D_READ_MOVE_LIMIT_RATIO)
        {
            stopVel(wheelVel);
            addMsg(ENUM_MSG_ERROR,"MotionServer","VehicleJogControl",22002,robotId,
                   vehicleVelAutoCommand.x_move,vehicleRearRunSpeedLimit);
            return -11;
        }
        if(1!=moveLoop_steeringSmooth(vehicleVelAutoCommand,wheelVel,steeringAngleOut))
        {
            return -10;
        }
    }
    else if(E_VEHICLE_MODE_JOG==mode)
    {
        VehicleVelocity vehicleVelJogCommand_safe;
        vehicleVelJogCommand_safe.x_move=vehicleVelJogCommand.x_move*jogSafeRatio;
        vehicleVelJogCommand_safe.y_move=vehicleVelJogCommand.y_move*jogSafeRatio;
        vehicleVelJogCommand_safe.z_rotate=vehicleVelJogCommand.z_rotate*jogSafeRatio;
        if(1!=moveLoop_steeringSmooth(vehicleVelJogCommand_safe,wheelVel,steeringAngleOut))
        {
            return -11;
        }

//        qDebug()<<"E_VEHICLE_MODE_JOG==mode, wheelVel"<<wheelVel;//yuguang todo
    }

    if(1!=speedLimitCheck(wheelVel))
    {
        stopVel(wheelVel);

        return -3;
    }

    return   1;

}

int VehicleJogControl::stopVel(QVector<double> &wheelVel)
{
    switch(vehicletype)
    {
        case E_VEHICLE_TYPE_TWO_DIFFERENTIAL_DRIVE:
        {
            wheelVel.resize(2);
            wheelVel[0]=0;
            wheelVel[1]=0;
            break;
        }
    case E_VEHICLE_TYPE_MCCRUM_4_WHEEL:
    {
        wheelVel.resize(4);
        wheelVel[0]=0;
        wheelVel[1]=0;
        wheelVel[2]=0;
        wheelVel[3]=0;
        break;
    }
    case E_VEHICLE_TYPE_ONE_STERRING_WHEEL:
    {
        wheelVel.resize(1);
        wheelVel[0]=0;
        break;
    }
    case E_VEHICLE_TYPE_TWO_STERRING_WHEEL:
    {
        wheelVel.resize(2);
        wheelVel[0]=0;
        wheelVel[1]=0;
        break;
    }
        default:
        {
        return -1;
        }
    }
    return 1;

}

int VehicleJogControl::getWheelData(std::vector<double> &wheelMoveSpeed, std::vector<double> &wheelPathLength)
{
    switch(vehicletype)
    {
        case E_VEHICLE_TYPE_TWO_DIFFERENTIAL_DRIVE:
        {
            wheelMoveSpeed.resize(2);
            wheelMoveSpeed[0]=wheelVelFeedback[0];
            wheelMoveSpeed[1]=wheelVelFeedback[1];

            wheelPathLength.resize(2);
            wheelPathLength[0]=wheelPosFeedback[0];
            wheelPathLength[1]=wheelPosFeedback[1];

            break;
        }
        default:
        {
        return -1;
        }
    }
    return 1;
}

int VehicleJogControl::getOdometer(VehicleVelocity &vehicleVelFeedbackOut,
                                   VehiclePosition &vehiclePositionFeedbackOut)
{
    if(false==initialFlag)
    {
        return -1;
    }

    vehicleVelFeedbackOut=vehicleVelFeedback;
    vehiclePositionFeedbackOut=vehiclePositionFeedback;
    return 1;

}

int VehicleJogControl::recordWheelFeedback(QVector<double> wheelVelFeedbackIn, QVector<double> wheelPosFeedbackIn,
                                           QVector<double> stteringAngleIn)
{
    for(int i=0;i<wheelVelFeedbackIn.size();i++)
    {
        wheelVelFeedback[i]=wheelVelFeedbackIn[i];
        wheelPosFeedback[i]=wheelPosFeedbackIn[i];
    }
    for(int i=0;i<stteringAngleIn.size();i++)
    {
        steeringAngleFeedback[i]=stteringAngleIn[i];
    }
    return 1;
}

int VehicleJogControl::odometerCalculationLoop(QVector<double> wheelVelFeedback,
                                               QVector<double> wheelPosFeedback,QVector<double>steeringAngle)
{
    if(false==initialFlag)
    {
        return -1;
    }

    vehicleKinematic->setSteeringPositionFeedback(steeringAngle);
    recordWheelFeedback(wheelVelFeedback,wheelPosFeedback,steeringAngle);

    //first to record
    if(false==timeStampInitialed)
    {
        clock_gettime(CLOCK_MONOTONIC , &timeStamp);
//        VehicleVelocity tmpVelFeedback;
//        int resultKey=vehicleKinematic->transWheelVel2VehicleVel(wheelVelFeedback,steeringAngle,
//                                                                 vehicleVelFeedback);
//        if(1!=resultKey)
//        {
//            return -2;
//        }

//        vehicleVelFeedback.x_move=filtering_odom_vx->arithmeticMeanfilter(tmpVelFeedback.x_move,5);
//        vehicleVelFeedback.y_move=filtering_odom_vx->arithmeticMeanfilter(tmpVelFeedback.y_move,5);
//        vehicleVelFeedback.z_rotate=filtering_odom_vx->arithmeticMeanfilter(tmpVelFeedback.z_rotate,5);
        timeStampInitialed=true;
        return 1;
    }

    //根据车轮速度计算的车体速度
    int resultKey=vehicleKinematic->transWheelVel2VehicleVel(wheelVelFeedback,steeringAngle,vehicleVelFeedback);
    if(1!=resultKey)
    {
        return -2;
    }

    //other times

//    double timeInterval=getTimeInterval(timeStamp);
    double timeInterval=deltaTime;
    if(imuDevice->isUseThisModule())
    {
        ImuDataInfo dataOut;
        if(1==imuDevice->receiveImuDeviceData(dataOut))
        {
            vehiclePositionFeedback.rotate=dataOut.yaw*M_PI/180.0;
            //速度进行打滑判断
            double tmpWheel=180.0*vehicleVelFeedback.z_rotate/M_PI;
            double tmpVelBias=tmpWheel-dataOut.rotateVel_z;
            if(fabs(tmpVelBias)>10)//10degree/s
            {
                addMsg(ENUM_MSG_WARNING,"MotionServer","VehicleJogControl",22008,robotId,tmpVelBias);
            }
            qDebug()<<"tmpVelBias"<<tmpVelBias<<"车轮"<<tmpWheel<<"陀螺仪"<<dataOut.rotateVel_z;
        }
        else
        {
            addMsg(ENUM_MSG_ERROR,"MotionServer","VehicleJogControl",22007,robotId);
        }

    }

    static int count=0;
    count++;
    double addx=(vehicleVelFeedback.x_move*cos(vehiclePositionFeedback.rotate)
                 - vehicleVelFeedback.y_move*sin(vehiclePositionFeedback.rotate)) * timeInterval;
//    qDebug()<<"count"<<count<<"x_move  "<<vehiclePositionFeedback.x_move <<"addx"<<addx;


    vehiclePositionFeedback.x+=addx;

    vehiclePositionFeedback.y+=(vehicleVelFeedback.x_move*sin(vehiclePositionFeedback.rotate)
            + vehicleVelFeedback.y_move*cos(vehiclePositionFeedback.rotate)) * timeInterval;

    if(false==imuDevice->isUseThisModule())
    {
        vehiclePositionFeedback.rotate+=vehicleVelFeedback.z_rotate * timeInterval;
    }


//    int resultKey=vehicleKinematic->transWheelVel2VehicleVel(wheelVelFeedback,steeringAngle,vehicleVelFeedback);
//    if(1!=resultKey)
//    {
//        return -2;
//    }

    return 1;
}

int VehicleJogControl::initialSteeringAxis()
{
    //轴初始化
    JointAttribute attribute;
    //trap用
    attribute.maxAccelation=steeringMaxAcceration;
    attribute.maxVelocity=steeringMaxSpeed;


    attribute.maxTorque=10000;
    attribute.maxPosition=10000.1;
    attribute.minPosition=-10000.1;
    attribute.coupleFlag=0;
    attribute.coupleJoint=0;
    attribute.coupleReducer=0;
    attribute.jointType=0;
    attribute.pulsePerRound=100;
    attribute.externAxisFlag=0;
    attribute.gearRatio=1;
    attribute.jointName="steeringAxis";
    attribute.mobile=false;
    attribute.processFollowError=2;
    attribute.screwPitch=100;
    attribute.targetFollowError=100;
    attribute.axisLogicalType=E_AXIS_LOGIC_TYPE_VIRTUAL_STAND_ALONE;

    steeringAxis=new AxisObject(1,attribute,motionMessage);
    steeringAxis->axisIdByRobot=1;
    steeringAxis->deltaTime=deltaTime;

    return 1;
}

int VehicleJogControl::moveLoop_steeringSmooth(VehicleVelocity vehicleVelCommand_safe,QVector<double> &wheelVel,
                                               QVector<double> &steeringAngleOut)
{
    steeringAxis->positionFeedback=steeringAngleFeedback[0];

    QVector<double> tmpWheelVel;
    QVector<double> tmpSteeringAngle;

    int tmpResult=vehicleKinematic->transVehicleVel2WheelVel(mode,vehicleVelCommand_safe,
                                                             tmpSteeringAngle,tmpWheelVel);
    if(1!=tmpResult)
    {
        stopVel(wheelVel);
//        addMsg(ENUM_MSG_ERROR,"MotionServer","VehicleJogControl",22000,robotId);
//        qDebug()<<"warnning,,,transVehicleVel2WheelVel failed,速度为０，转角不变";
        return tmpResult;
    }
    switch(vehicletype)
    {
        case E_VEHICLE_TYPE_TWO_DIFFERENTIAL_DRIVE:
        {
            wheelVel=tmpWheelVel;
            return 1;
            break;
        }
    case E_VEHICLE_TYPE_MCCRUM_4_WHEEL:
    {
        wheelVel=tmpWheelVel;
        return 1;
        break;
    }
    case E_VEHICLE_TYPE_ONE_STERRING_WHEEL:
    {
        wheelVel=tmpWheelVel;
        steeringAngleOut=tmpSteeringAngle;
        return 1;
        break;
    }
    case E_VEHICLE_TYPE_NONE://因为手动命令滤波，导致多次过渡平滑。
    {
        if(isInSteeringSmooth)//转角过渡中
        {
            int tmpKey=steeringAxis->moveOneStep();
            if(1==tmpKey)
            {
                wheelVel.clear();//驱动线速度保持旧值不变
                steeringAngleOut.resize(1);
                steeringAngleOut[0]=steeringAxis->positionCommand;
                return 1;
            }
            else
            {
                addMsg(ENUM_MSG_REMIND,"MotionServer","VehicleJogControl",22004,robotId);
                qDebug()<<"steering smooth finished!!! tmpKey"<<tmpKey;
                isInSteeringSmooth=false;
                return 1;
            }


        }
        else//非转角过渡中
        {
            //转角偏差大于设定值degree，且轮速度小于设定值degree/s
            if(fabs(tmpSteeringAngle[0]-steeringAngleFeedback[0])>steeringAngleOffset_wait
                    &&  wheelVelFeedback[0]<5)
            {
                steeringAxis->resetAll();//保证立刻覆盖前面的旧轨迹,resetTraj没有复位命令值
                steeringAxis->trapMove(tmpSteeringAngle[0],1);
                isInSteeringSmooth=true;
                addMsg(ENUM_MSG_REMIND,"MotionServer","VehicleJogControl",22003,robotId);
                qDebug()<<"steering smooth start!!! ";
                return 1;
            }
            else
            {

                wheelVel=tmpWheelVel;
                steeringAngleOut=tmpSteeringAngle;
                return 1;
            }
        }



        break;
    }
    case E_VEHICLE_TYPE_TWO_STERRING_WHEEL:
    {

        break;
    }
        default:
        {
        return -1;
        }
    }
    return 1;

}

int VehicleJogControl::jogFitteringLoop()
{

    if(NULL!=velocitySmoother)
    {
        velocitySmoother->setAgvVelFeedback(vehicleVelFeedback);
        if(E_VEHICLE_MODE_AUTO==mode)
        {
            velocitySmoother->calculateSmootherVelocityCommand(vehicleVelAutoCommand);
        }
        else
        {
            //平滑滤波
            vehicleVelJogCommand.z_rotate=filtering_jog_r->arithmeticMeanfilter_low(vehicleVelJogCommand_beforeFilter.z_rotate,jogAcceleration_rotate);
            vehicleVelJogCommand.x_move=filtering_jog_x->arithmeticMeanfilter_low(vehicleVelJogCommand_beforeFilter.x_move,jogAcceleration_line);
            vehicleVelJogCommand.y_move=filtering_jog_y->arithmeticMeanfilter_low(vehicleVelJogCommand_beforeFilter.y_move,jogAcceleration_line);
//            velocitySmoother->calculateSmootherVelocityCommand(vehicleVelJogCommand);
        }

    }

    return 1;
}




int VehicleJogControl::readConfigFile(QString filePathIn,QString &readComent)
{
    QVariant strConfigValue;
    QString firstTimeFlagStr = filePathIn;
    QString strConfigGroup = "";
    QString strConfigKey = "";

    QSettings settings(firstTimeFlagStr,QSettings::IniFormat);
    settings.setIniCodec("UTF-8");
    strConfigGroup = "MechanicalInfo";
    settings.beginGroup(strConfigGroup);

    //vehicletype
    strConfigKey = "vehicletype";
    if(false==settings.contains(strConfigKey))
    {
        return -1;
    }
    strConfigValue = settings.value(strConfigKey);
    vehicletype= (E_VEHICLE_TYPE)strConfigValue.toInt();

    //wheelDiameter
    strConfigKey = "wheelDiameter";
    if(false==settings.contains(strConfigKey))
    {
        return -2;
    }
    strConfigValue = settings.value(strConfigKey);
    wheelDiameter= strConfigValue.toDouble();

    //correctionFactor_wheelDiameter
    strConfigKey = "correctionFactor_wheelDiameter";
    if(false==settings.contains(strConfigKey))
    {
        return -2;
    }
    strConfigValue = settings.value(strConfigKey);
    correctionFactor_wheelDiameter= strConfigValue.toDouble();


    //wheelDistance
    strConfigKey = "wheelDistance";
    if(false==settings.contains(strConfigKey))
    {
        return -3;
    }
    strConfigValue = settings.value(strConfigKey);
    wheelDistance= strConfigValue.toDouble();

    //correctionFactor_wheelDistance
    strConfigKey = "correctionFactor_wheelDistance";
    if(false==settings.contains(strConfigKey))
    {
        return -3;
    }
    strConfigValue = settings.value(strConfigKey);
    correctionFactor_wheelDistance= strConfigValue.toDouble();

    //wheelDistance_height
    strConfigKey = "wheelDistance_height";
    if(false==settings.contains(strConfigKey))
    {
        return -3;
    }
    strConfigValue = settings.value(strConfigKey);
    wheelDistance_height= strConfigValue.toDouble();

    //correctionFactor_wheelDistance_height
    strConfigKey = "correctionFactor_wheelDistance_height";
    if(false==settings.contains(strConfigKey))
    {
        return -3;
    }
    strConfigValue = settings.value(strConfigKey);
    correctionFactor_wheelDistance_height= strConfigValue.toDouble();


    //wheelMaxSpeed
    strConfigKey = "wheelMaxSpeed";
    if(false==settings.contains(strConfigKey))
    {
        return -4;
    }
    strConfigValue = settings.value(strConfigKey);
    wheelMaxSpeed= strConfigValue.toDouble();

    //maxSpeedX
    strConfigKey = "maxSpeedX";
    if(false==settings.contains(strConfigKey))
    {
        return -5;
    }
    strConfigValue = settings.value(strConfigKey);
    vehicleMaxVel.x_move= strConfigValue.toDouble();



    //maxSpeedY
    strConfigKey = "maxSpeedY";
    if(false==settings.contains(strConfigKey))
    {
        return -6;
    }
    strConfigValue = settings.value(strConfigKey);
    vehicleMaxVel.y_move= strConfigValue.toDouble();

    //maxSpeedRotation
    strConfigKey = "maxSpeedRotation";
    if(false==settings.contains(strConfigKey))
    {
        return -7;
    }
    strConfigValue = settings.value(strConfigKey);
    vehicleMaxVel.z_rotate= strConfigValue.toDouble();

    //vehicleRearRunSpeedLimit
    strConfigKey = "vehicleRearRunSpeedLimit";
    if(false==settings.contains(strConfigKey))
    {
        return -8;
    }
    strConfigValue = settings.value(strConfigKey);
    vehicleRearRunSpeedLimit= strConfigValue.toDouble();

    //jogAcceleration_rotate
    strConfigKey = "jogAcceleration_rotate";
    if(false==settings.contains(strConfigKey))
    {
        return -9;
    }
    strConfigValue = settings.value(strConfigKey);
    jogAcceleration_rotate= strConfigValue.toDouble();

    //jogAcceleration_line
    strConfigKey = "jogAcceleration_line";
    if(false==settings.contains(strConfigKey))
    {
        return -10;
    }
    strConfigValue = settings.value(strConfigKey);
    jogAcceleration_line= strConfigValue.toDouble();

    //filter_maxVel_line
    strConfigKey = "filter_maxVel_line";
    if(false==settings.contains(strConfigKey))
    {
        return -10;
    }
    strConfigValue = settings.value(strConfigKey);
    filter_maxVel_line= strConfigValue.toDouble();

    //filter_maxVel_rotate
    strConfigKey = "filter_maxVel_rotate";
    if(false==settings.contains(strConfigKey))
    {
        return -10;
    }
    strConfigValue = settings.value(strConfigKey);
    filter_maxVel_rotate= strConfigValue.toDouble();


    //filter_maxAcc_line
    strConfigKey = "filter_maxAcc_line";
    if(false==settings.contains(strConfigKey))
    {
        return -10;
    }
    strConfigValue = settings.value(strConfigKey);
    filter_maxAcc_line= strConfigValue.toDouble();


    //filter_maxAcc_rotate
    strConfigKey = "filter_maxAcc_rotate";
    if(false==settings.contains(strConfigKey))
    {
        return -10;
    }
    strConfigValue = settings.value(strConfigKey);
    filter_maxAcc_rotate= strConfigValue.toDouble();


    //filter_decel_factor
    strConfigKey = "filter_decel_factor";
    if(false==settings.contains(strConfigKey))
    {
        return -10;
    }
    strConfigValue = settings.value(strConfigKey);
    filter_decel_factor= strConfigValue.toDouble();

    //steeringAngleOffset_wait
    strConfigKey = "steeringAngleOffset_wait";
    if(false==settings.contains(strConfigKey))
    {
        return -11;
    }
    strConfigValue = settings.value(strConfigKey);
    steeringAngleOffset_wait= strConfigValue.toDouble();

    //steeringMaxSpeed
    strConfigKey = "steeringMaxSpeed";
    if(false==settings.contains(strConfigKey))
    {
        return -12;
    }
    strConfigValue = settings.value(strConfigKey);
    steeringMaxSpeed= strConfigValue.toDouble();

    //steeringMaxAcceration
    strConfigKey = "steeringMaxAcceration";
    if(false==settings.contains(strConfigKey))
    {
        return -13;
    }
    strConfigValue = settings.value(strConfigKey);
    steeringMaxAcceration= strConfigValue.toDouble();

    //steeringNoMove_rVel
    strConfigKey = "steeringKp";
    if(false==settings.contains(strConfigKey))
    {
        return -14;
    }
    strConfigValue = settings.value(strConfigKey);
    steeringKp= strConfigValue.toDouble();
//    steeringKp=steeringKp*M_PI/180.0;

    strConfigKey = "maxSteerAdjust";
    if(false==settings.contains(strConfigKey))
    {
        readComent+=" maxSteerAdjust";
        return -15;
    }
    strConfigValue = settings.value(strConfigKey);
    maxSteerAdjust= strConfigValue.toDouble();


    settings.endGroup();

    qDebug() << "vehicleJogControl::readConfigFile sucess";
    return 1;
}

int VehicleJogControl::writeConfigFile(QString filePathIn, double correctionFactor_wheelDiameterIn,
                                       double correctionFactor_wheelDistanceIn,
                                       double correctionFactor_wheelDistance_heightIn)
{

    qDebug()<<"writeConfigFile,fileName="<<filePathIn;
    QSettings settings(filePathIn, QSettings::IniFormat);

    settings.setValue("MechanicalInfo/correctionFactor_wheelDiameter",QVariant::fromValue(correctionFactor_wheelDiameterIn));
    settings.setValue("MechanicalInfo/correctionFactor_wheelDistance",QVariant::fromValue(correctionFactor_wheelDistanceIn));
    settings.setValue("MechanicalInfo/correctionFactor_wheelDistance_height",QVariant::fromValue(correctionFactor_wheelDistance_heightIn));

    return 1;

}

int VehicleJogControl::speedLimitCheck(QVector<double> wheelVel)
{
    for(int i=0;i<wheelVel.size();i++)
    {
        if(fabs(wheelVel[i])>wheelMaxSpeed)
        {
            addMsg(ENUM_MSG_ERROR,"MotionServer","VehicleJogControl",22001,robotId,wheelVel[i],wheelMaxSpeed);
//            wheelVel[i]=0;
            return -1;
        }
    }
    return 1;
}

double VehicleJogControl::getTimeInterval(timespec &lastTime)
{

    struct timespec time_now;
    clock_gettime(CLOCK_MONOTONIC , &time_now);
    double timeInterval;
    timeInterval=time_now.tv_sec-lastTime.tv_sec + (time_now.tv_nsec-lastTime.tv_nsec)/1000000000.0;
    lastTime=time_now;
    return timeInterval;

}


void VehicleJogControl::addMsg(int messageLevel, std::string componentName, std::string messageType
                              , int messageCode, int robotIdIn
                              , double parameter1, double parameter2, int parameter3, int parameter4, QString comment)
{
    Message tmpMsg;
    tmpMsg.MessageLevel = messageLevel;
    tmpMsg.componentClassName = componentName;
    tmpMsg.messageCode = messageCode;
    tmpMsg.messageType = messageType;
    tmpMsg.robotId = robotIdIn;

    string infomationStr;
    QString tmpStr;

    switch(messageCode)
    {
    case 22008:
    {
        tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
        tmpStr = tmpStr + QObject::tr(",检测到车轮打滑，打滑速度(%1)！").arg(parameter1);
        infomationStr = tmpStr.toStdString();
        break;
    }
    case 22007:
    {
        tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
        tmpStr = tmpStr + QObject::tr(",imu设备通讯超时！");
        infomationStr = tmpStr.toStdString();
        break;
    }
    case 22006:
    {
        tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
        tmpStr = tmpStr + QObject::tr(",imu设备初始化失败！");
        infomationStr = tmpStr.toStdString();
        break;
    }
    case 22005:
    {
        tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
        tmpStr = tmpStr + QObject::tr(",车手动控制模块读取配置文件失败！")+comment;
        infomationStr = tmpStr.toStdString();
        break;
    }
    case 22004:
    {
        tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
        tmpStr = tmpStr + QObject::tr(",转向轮平滑过渡结束！");
        infomationStr = tmpStr.toStdString();
        break;
    }
    case 22003:
    {
        tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
        tmpStr = tmpStr + QObject::tr(",转向轮平滑过渡开始！");
        infomationStr = tmpStr.toStdString();
        break;
    }
    case 22002:
    {
        tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
        tmpStr = tmpStr + QObject::tr(",后退速度(%1)超过允许值(%2)！").arg(parameter1).arg(parameter2);
        infomationStr = tmpStr.toStdString();
        break;
    }

    case 22001:
    {
        tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
        tmpStr = tmpStr + QObject::tr(",车轮速度(%1)超过最大允许速度(%2)！！").arg(parameter1).arg(parameter2);
        infomationStr = tmpStr.toStdString();
        break;
    }

    case 22000:
    {
        tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
        tmpStr = tmpStr + QObject::tr(",车速 转换 轮速度失败！");
        infomationStr = tmpStr.toStdString();
        break;
    }



        default:
        {
            qDebug()<<"Coordinate::addMsg====not match error code";
            break;
        }
    }

    tmpMsg.MessageInformation = infomationStr;

    #ifndef MOTION_SERVER_UNIT_TEST
    motionMessage->addMsg(tmpMsg);
    #endif
}


